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Whole  Arm  Grasping  Control  for  Redundant  Robot  Manipulators 

D.  Braganza,  M.  L.  McIntyre,  D.  M.  Dawson  and  I.  Walker 


Abstract — An  approach  to  whole  arm  grasping  of  objects 
using  redundant  robot  manipulators  is  presented.  A  kinematic 
control  development  is  presented  which  facilitates  the  encoding 
of  both  the  end-effector  position,  as  well  as  body  self-motion 
positioning  information  as  a  desired  trajectory  signal  for  the 
manipulator  joints.  A  joint  space  controller  which  provides 
asymptotic  tracking  of  the  encoded  desired  trajectory  in  the 
presence  of  system  uncertainties  is  then  presented.  Experimen¬ 
tal  results  for  a  planar,  three  link  configuration  of  the  Barrett 
whole  arm  manipulator  are  provided  to  illustrate  the  validity 
of  the  approach. 

I.  Introduction 

One  of  the  advantages  of  redundant  robot  manipulators  is 
their  ability  to  perform  whole  arm  grasping  of  objects.  Whole 
arm  manipulation  [21]  is  the  term  used  to  describe  the  ability 
of  the  manipulator  to  grasp  an  object  with  its  entire  body 
(or  arm),  as  compared  to  fingertip  grasping  performed  by 
traditional  robotic  grippers  and  hands.  Whole  arm  grasping1 
can  be  performed  by  allowing  the  robot  manipulator  to  make 
contact  with  the  object  in  a  snake  or  tentacle  like  manner, 
using  portions  of  the  manipulator  itself  to  wrap  around 
the  object  and  grasp  it.  The  equivalent  whole  hand  and 
whole  finger  grasping  techniques  have  been  studied  in  [14] 
and  [20],  respectively.  Whole  arm  grasping  is  also  known 
by  the  equivalent  expressions  “power  grasping’’  ([15]  and 
[22])  or  “enveloping  grasping’’  [25].  Recently  in  [19],  the 
authors  presented  experimental  results  which  demonstrated 
whole  hand  grasping  with  a  12  degree-of-freedom  (DOF) 
robotic  hand.  However,  there  has  been  very  little  experimen¬ 
tal  work  reported  on  whole  arm  grasping  with  redundant 
robot  manipulators.  Specifically,  one  of  the  few  results  in  the 
literature  is  given  in  [7]  where  whole  arm  grasping  with  a  30 
DOF  robotic  arm  is  demonstrated.  Shape  control  is  another 
technique  which  is  being  studied  for  the  control  of  whole  arm 
grasping.  In  [16],  the  author  proposed  an  impedance  control 
based  approach  to  control  the  shape  of  the  whole  arm  of  a 
redundant  manipulator.  Whole  arm  grasping  has  a  number  of 
useful  properties  as  noted  by  [1],  [15],  [21],  and  others.  The 
authors  of  [15]  point  out  that  distribution  of  contact  points 
enables  increased  load  capacity.  The  ability  to  use  the  entire 
body  of  the  manipulator  for  grasping  also  allows  objects  of 
various  dimensions  to  be  grasped  [21].  These  capabilities  can 
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be  used  in  many  applications,  including,  search  and  rescue, 
underwater  and  space  exploration. 

Traditional  robotic  grasping  control  can  be  broadly  classi¬ 
fied  into  two  main  categories  [18].  The  first  category,  known 
as  the  geometrical  planning  based  approach,  requires  the 
object  model  and  the  constraint  forces  to  be  known  a  priori 
(e.g.  [1]  and  [23]).  Here,  the  grasping  contact  points  are  pre¬ 
planned  and  the  desired  constraint  force  for  each  contact 
point  are  assumed  to  be  known.  The  grasping  control  system 
then  moves  the  hand/arm  along  a  pre-determined  trajectory 
and  force  feedback  (force  sensors  on  the  arm/hand)  is  used 
to  control  the  interaction  forces.  The  second  category  for 
robot  grasping  control  is  the  sensory  approach,  where  the 
object  model  is  unknown  and  the  grasping  controller  relies  on 
tactile  force-feedback  data.  In  this  sensory  based  approach, 
it  is  often  assumed  that  the  arm  has  a  sensory  “skin’’  for 
force  measurements  [2].  The  arm/hand  must  either  start  off 
close  to  the  object  to  be  grasped,  or  with  all  contact  points 
touching  the  object.  Then,  the  grasp  controller  positions  and 
re-positions  the  arm  to  minimize  an  error  function  in  an 
attempt  to  optimize  the  grasp  configuration  [19]. 

The  techniques  described  above  require  either  that  the 
geometry  of  the  object  and  the  constraint  forces  be  known 
a  priori  [23],  or  that  the  contact  forces  be  measurable  using 
some  type  of  force  sensor  [1],  [2],  and  [18].  When  extending 
the  traditional  approaches  (i.e.,  fingertip  grasping)  to  whole- 
arm  grasping,  the  previously  mentioned  requirements  might 
not  be  easily  met  due  to  the  increased  number  of  contact 
points  and  the  large  number  of  grasping  configurations 
possible  [19].  Motivated  by  the  need  to  have  a  whole  arm 
grasping  controller  which  does  not  require  the  constraint 
forces  to  be  known  a  priori  while  also  eliminating  the 
requirement  for  contact  force  sensing  (due  to  the  inherent 
inaccuracy/noise  in  measurement),  a  grasping  controller  for 
redundant  robot  manipulators  is  designed  which  requires 
only  the  object  geometry  to  be  known  a  priori.  In  addition, 
the  proposed  controller  does  not  require  the  exact  dynamic 
model  for  the  robot  manipulator  or  the  contact  forces.  This 
paradigm  makes  the  whole  arm  grasping  technique  easily 
extendable  to  various  manipulator  systems. 

Roughly  speaking,  the  whole  arm  grasping  objective  is 
achieved  by  integrating  the  path  planner  and  the  controller 
such  that  two  tasks,  robot  end-effector  positioning  and  robot 
body  self-motion  positioning,  are  accomplished  simultane¬ 
ously.  The  end-effector  positioning  controller  forces  the  end- 
effector  to  follow  a  path  around  the  object  which  in  turn, 
forces  the  robot’s  body  to  wrap  itself  around  the  object  to  be 
grasped.  The  body  self-motion  positioning  controller  “repels” 
the  body  of  the  manipulator  away  from  the  object  while  the 


end-effector  moves  around  the  object.  This  control-induced 
repulsion-like  property  facilitates  object  avoidance  as  well  as 
removes  the  “slack”  from  the  robot  body  as  the  robot  begins 
to  move  into  the  grasping  position.  When  all  possible  slack 
is  removed,  the  manipulators  body  makes  contact  with  the 
object,  hence,  completing  the  whole  arm  grasp  of  the  object. 

To  facilitate  the  explanation  of  the  proposed  whole  arm 
grasping  control  design,  we  first  develop  a  Lyapunov-based 
kinematic  control.  The  kinematic  control  input  is  then  passed 
through  a  desired  trajectory  filter  which  produces  a  desired, 
joint  level  trajectory.  The  smooth  control  strategy  developed 
in  [26]  is  then  utilized  for  the  joint  space  controller  since  it 
provides  asymptotic  tracking  of  the  desired  trajectory  in  the 
presence  of  dynamic  uncertainties.  Experimental  results  for  a 
planar  application  of  the  whole  arm  grasping  technique  using 
the  Barrett  whole  arm  manipulator  are  provided  to  illustrate 
the  performance  of  the  controller. 


Property  1:  The  inertia  matrix  M(q)  is  symmetric  and 
positive-definite,  and  satisfies  the  following  inequalities 

mi  ||£||2  <  eM(q)£  <  m2  ||£||2  €  IT  (4) 

where  mi,  m2  G  M  are  positive  constants,  and  ||-||  denotes 
the  standard  Euclidean  norm. 

Remark  1:  Since  this  development  is  only  concerned  with 
revolute  robot  manipulators,  the  kinematic  and  dynamic 
terms  denoted  by  M{q),  N{q,q),  and  J{q),  are  assumed 
to  be  bounded  for  all  possible  q(t)  ( i.e.,  these  kinematic 
and  dynamic  terms  only  depend  on  q{t)  as  arguments  of 
trigonometric  functions ). 

Ill.  Grasping  with  Kinematic  Control 

To  facilitate  the  kinematic  control  development,  the 
pseudo-inverse  of  Jn(q)  denoted  by  Jf{q)  G  Rnxp,  is 
defined  as  follows 


II.  Kinematic  and  Dynamic  Models 
In  this  section  the  kinematic  and  dynamic  models  for  an 
//-joint  (n  >  6),  revolute,  direct  drive  robot  manipulator  are 
presented.  The  subsequent  development  is  based  on  these 
models. 

A.  Kinematic  Model 

The  Denavit-Hartenberg  based  forward  kinematic  model 
for  an  /(-segment  redundant  manipulator  can  be  developed 
as  follows 

xn  =  fn(q)  (1) 


7+  A  tT  t  j  tT\  1 
dn  ~  °n  \un^n  ) 

where  J+  (q)  satisfies  the  following  equality 

°n°  n  —  -*• p 


(5) 

(6) 


where  Ip  G  Rpxp  is  the  standard  identity  matrix.  As  shown 
in  [17],  the  pseudo-inverse  defined  by  (5)  satisfies  the  Moore  - 
Penrose  conditions  given  below 


7  7+7  —  7 
(#  Jnf  =  J+Jn 


j+  j  /+  —  /+ 
°n  ~  °n 

( J„  J+)T  =  JnJ+. 


where  xn(t)  G  Mp  represents  the  robot  end-effector’s  task- 
space  vector,  q(t)  G  1"  denotes  the  joint  position,  and 
fn{q)  G  Rp  denotes  the  forward  kinematics  of  the  manip¬ 
ulator.  The  velocity  kinematics  for  the  manipulator  can  be 
developed  as  follows 

in  =  Jn(q)q(t)  (2) 

where  xn(t)  G  Rp  represents  the  task-space  velocity,  q(t)  G 

R”  denotes  the  joint  velocity,  and  Jn(q )  =  g  RPX" 

oq 

denotes  the  manipulator  Jacobian. 

B.  Dynamic  Model 

The  dynamic  model  for  an  //-joint  in  >  6),  revolute, 
direct  drive  robot  manipulator  is  described  by  the  following 
expression  [8] 

M(q)q  + N(q,q)  +  Fe(q,q)  =  t  (3) 

where  M(q)  G  R"xrl  represents  the  inertia  effects,  N  (q.  q)  G 
R"  represents  the  remaining  dynamic  terms,  such  as  the 
centripetal-Coriolis  effects,  gravitational  effects,  and  fric¬ 
tional  effects,  Fe  ( q ,  q)  G  Rn  represents  the  contact  forces 
placed  on  the  robot  manipulator  by  the  environment,  r(7)  G 
R"  represents  the  input  torque  vector.  The  subsequent  de¬ 
velopment  is  based  on  the  assumptions  that  q(t)  and  q(t) 
are  measurable,  M(q),  N(q,q),  and  Fe(q,q)  are  unknown, 
second  order  differentiable,  functions  of  q(t)  and  q(t),  and 
the  following  property  holds  [8], 


In  addition  to  the  above  properties,  the  matrix  ( —  .Jf ) 
satisfies  the  following  useful  properties 


{In  ~  J^Jn)  {In  ~  Jn  Jn)  —  In  Jn  Jn 
{In  -  J+Jnf  =  {In  ~  J+  Jn) 

Jn  {In  -  J£Jn)  =  0 
{In  -  J+Jn)  J+  =  0 


(8) 


where  In  G  R™xn  is  the  standard  identity  matrix. 

Remark  2:  During  the  control  development,  the  assump¬ 
tion  is  made  that  the  minimum  singular  value  of  the  manip¬ 
ulator  Jacobian,  denoted  by  am  A  greater  than  a  known, 
small  positive  constant  5  >  0,  such  that  max{||  J+(g)||}  is 
known  a  priori  and  all  kinematic  singularities  are  always 
avoided. 

Typically  in  the  robotics  literature,  when  a  kinematic 
control  is  designed,  q{t)  is  taken  to  be  the  control  input. 
A  joint  space  controller  must  then  be  used  to  ensure  that 
the  actual  robot  joint  angles  track  this  reference  trajectory. 
Following  this  paradigm,  the  kinematic  controller  is  first 
designed  as  follows 


q{t)  4  J+Ue  +  {In  -  J+Jn )  Um  (9) 


where  Ue  (7)  G  Rp  is  the  end-effector  positioning  controller, 
and  Um(t)  G  Rn  is  the  robot  body  self-motion  controller. 
In  the  subsequent  sub-sections,  the  design  of  the  robot  end- 
effector  positioning  controller  Ue{t)  and  the  robot  body  self- 
motion  controller  Um{t)  will  be  discussed  in  detail. 


A.  End-Effector  Positioning 

The  objective  of  the  end-effector  positioning  controller  is 
to  force  the  end-effector  to  track  a  desired  trajectory  that 
encompasses  the  surface  of  the  object  to  be  grasped.  For 
this  type  of  problem,  instead  of  a  time  based  trajectory, 
a  velocity  field  control  (VFC)  is  utilized  because  it  more 
effectively  penalizes  the  end-effector  for  leaving  the  contour 
([6],  [9],  and  [10]).  The  VFC  will  also  not  exhibit  the  radial 
reduction  phenomenon  which  is  common  with  traditional 
control  methods  ([6]  and  [9]).  For  example,  when  the  object 
to  be  grasped  is  circular,  the  velocity  field  generates  a  desired 
trajectory  that  forces  the  end-effector  to  spiral  inwards, 
toward  and  around  the  surface  of  the  object. 

Remark  3:  A  velocity  field  specifies  a  desired  velocity 
xd(t)  at  each  displacement  position  xnft)  on  the  task  space 
of  the  system  [9],  In  [9]  and  [10],  the  authors  provide 
specific  information  about  the  construction  of  velocity  fields. 
See  [6]  and  [12]  for  details  of  circular  velocity  fields.  The 
velocity  field  for  a  specific  planar  application  is  presented 
subsequently  in  the  Appendix. 

The  end-effector  positioning  controller  Ue(t)  £  Rp  is 
designed  as  follows 

2 

p2(xn,xd)e  (10) 

where  {)  (xn)  £  Rp  is  a  task-space  velocity  field,  Ke  £ 
Rpxp  is  a  positive  definite  diagonal  gain  matrix,  kn  £  R+ 
is  a  scalar  gain  parameter,  eft)  £  Rp  is  the  error  between 
the  desired  and  actual  task  space  position  and  is  defined  as 
follows 

e  =  xd-xn ,  (11) 

where  xd  (t)  £  Rp  is  the  desired  task-space  position,  and 
xn  ( t )  was  introduced  in  (1).  In  (10),  V(xd)  £  R  is  a  first 
order  differentiable,  nonnegative  function  and  p  (■)  £  R  is 
a  known  positive  function  that  is  assumed  to  be  bounded 
provided  xn(t)  and  xd(f)  are  bounded.  The  function  V(xd) 
is  defined  for  a  given  application  subsequently  in  Section 
I.  For  details  on  how  to  construct  p(xn,xd)  for  a  specific 
application,  the  reader  is  referred  to  [12], 

For  the  whole  arm  grasping  control  objective,  the  desired 
task  space  velocity  trajectory  is  defined  as 

Xd(t)=-&{  Xn)  (12) 

where  i9  (xn)  is  the  velocity  field  generated  by  the  task- 
space  position  xn(t).  The  velocity  tracking  error  signal  can 
be  derived  by  taking  the  first  derivative  of  (11)  and  using 
(12),  we  have 

e  =  &{xn)  —  xn.  (13) 

After  utilizing  (2),  the  expression  in  (13)  can  be  written  as 
follows 

e  =  fi(xn)  -  Jnq.  (14) 

After  utilizing  (9),  the  expression  in  (14)  can  be  written  as 
follows 

(15) 


Ue  =  ti  (xn)  +  Kee  +  kn 


dV{xd) 


dxd 


where  (6)  and  (8)  has  been  used.  After  substituting  for  Ue(t), 
as  defined  in  (10),  the  expression  in  (15)  can  be  written  as 
follows 


e  =  —Kee 


dV{xd) 

dxd 


2 

p2  (xn,xd)  e. 


(16) 


Theorem  1:  The  control  law  described  by  (10)  guarantees 
that  eft),  eft)  and  Ue(t)  £  Coo  and  ||e(£)||  — »  0  as  t.  — >  oo. 

Proof:  See  [12],  which  contains  a  similar  result.  ■ 
The  result  of  Theorem  1  proves  that  ||e(f)  ||  — ■»  0  as  t  — >  oo 
and  that  Ue(t)  £  Coo ■  Thus,  the  control  law  defined  in 
(10)  guarantees  that  the  manipulators  end-effector  follows  the 
desired  contour  while  also  ensuring  that  all  signals  remain 
bounded.  If  the  controller  defined  in  (10)  is  used  alone  (i.e. 
qft)  =  Jfi Ue),  the  joint  space  desired  trajectory  that  is 
tracked  may  take  a  path  such  that  the  end-effector  and  body 
of  the  manipulator  may  make  contact  with  the  object  while 
the  end-effector  tries  to  follow  the  contour  of  the  object  to 
be  grasped.  Since  this  is  an  undesirable  effect,  the  robot 
body  self-motion  positioning  controller  is  designed  in  such 
a  manner  that  provides  object  avoidance  as  the  body  of  the 
manipulator  moves  around  the  object  to  be  grasped. 


B.  Body  Self-Motion  Positioning 

The  objective  of  the  body  self-motion  positioning  con¬ 
troller  is  to  “repel”  the  end-effector  and  body  of  the  ma¬ 
nipulator  away  from  the  object  to  be  grasped,  while  the 
end-effector  moves  around  the  object.  This  control-induced 
repulsion-like  property  not  only  facilitates  obstacle  avoidance 
but  also  removes  the  “slack”  from  the  body  of  the  manip¬ 
ulator  as  the  robot  moves  into  the  grasping  position.  When 
all  possible  slack  is  removed,  the  manipulator  body  makes 
contact  with  the  object,  hence  completing  the  whole  arm 
grasp  of  the  object.  Following  this  line  of  reasoning,  the 
body  self-motion  positioning  controller  Umft)  £  R"  in  (9), 
is  designed  as  follows 

Um  4  -km  [Js  (In  -  J+Jn )]  T  Va  (17) 

where  km  £  R+  is  a  control  gain,  Js  £  Rlx"  is  a 
subsequently  designed  Jacobian-like  vector,  In  £  R"xn  was 
defined  in  (8),  and  ya  ft)  £  R  is  an  auxiliary  scalar  signal 
which  is  yet  to  be  defined.  The  signal  ya  ft)  encodes  the 
geometric  information  about  the  object’s  surface  and  how 
it  relates  to  the  manipulator’s  joint  positions  in  an  effort  to 
keep  the  body  of  the  manipulator  away  from  the  object.  See 
[24]  for  details  of  a  general  auxiliary  signal  for  self-motion 
control  of  a  redundant  robot  manipulator. 

For  whole  arm  grasping,  a  specific  auxiliary  signal  ya  ( t ) 
is  designed  as  follows 

n 

Va  =  F  Ki  (Xj)  (18) 

i—1 

where  n  is  the  number  of  joints  of  the  redundant  manipulator, 
Xi  =  [  Xn  Xi2  ■  ■  ■  XiP  ]  e  Rp  is  the  Euclidean- 
space  coordinate  for  the  ith  joint,  and  hai  {xf)  £  R  is  the 
repulsion  function  for  the  ith  joint  that  encodes  the  geometric 
information  about  the  surface  of  the  object  with  respect  to  the 


e  =  d(xn)  -  U( 


ith  joint’s  Euclidean  position.  The  repulsion  function  hai  (xf) 
is  defined  as  follows 

hai  (xi)  =  khiexp  (-ai/3?  (x,)) ,  Vi=  1, ..,  n  (19) 

where  khi,  on  £  R+  are  constants,  and  /3*  (xi)  £  R  is 
the  joint  specific  geometric  function.  The  function  /3,  (a;,) 
should  be  designed  to  be  positive  when  the  manipulator  is 
not  touching  the  object  as  well  as  that  /%  (a;,)  £  Coo,  if 
Xi(t)  £  Coo.  F°r  example,  given  a  spherical  object  in  three 
dimensional  Euclidean-space,  /%  (a;*)  could  be  defined  as 
follows 

Pi  (Xi)  =  (xn  -  xcl)2  +  (xi2  -  xc2)2  +  (xi3  -  xc3 )2  -  rl 

where  xc\,  xc2,  xc3 ,  r0  £  R  are  the  Euclidean  coordinates  of 
the  center  of  the  spherical  object  and  its  radius,  respectively. 

To  determine  the  dynamics  of  ya(t),  the  time  derivative 
of  (18)  is  taken,  and  can  be  written  as  follows 


Va  =  Jsq 

where  a  Jacobian-type  vector  Js(t)  £ 
follows 


is  defined  as 


dya  (  xi  x2 
d  xf  x\ 


where  ij=  Jiq  and  £  S.pxn  is  the  Jacobian  matrix 
relating  the  joint  velocities  and  the  Euclidean  velocities  for 
the  ith  joint.  Using  (9)  and  substituting  for  q(t)  in  (20),  the 
expression  for  ya(t)  can  be  written  as  follows 

ya  =  JsJ+Ue  +  Js  (In  -  J+Jn)  Um.  (22) 

After  substituting  for  Um(t)  as  defined  in  (17),  ya(t)  of  (22) 
can  be  further  expressed  as 

lla  km  1 1  Js  (hn  Jji  Jn)  1 1  ya  T"  3 SJ n  kk e  *  (73) 

Theorem  2:  The  control  law  described  by  (17)  guarantees 
that  ya(t)  is  practically  regulated  (i.e..  ultimately  bounded) 
in  the  following  sense 

If/a (t) |  <  G  \Va{to)\2  exp  (—2/it)  +  ^  (24) 

provided  the  following  sufficient  conditions  are  true 

||  Js(ln-J+J)\\2  >6  (25) 


km  >  YT  (26) 

002 

where  to,  fi,  5,  d2  £  R  are  positive  constants. 

Proof:  See  [24],  which  has  a  similar  result.  ■ 

Remark  4:  From  (18)  and  (19),  it  is  clear  that  0  < 
ya(t)  <  hhi  and  that  as  /?»(•)  increases,  hai(t) 

decreases,  and  hence,  ya(t)  decreases.  In  addition  each  (3 ,(•) 
is  designed  such  that  (3 ,(•)  >  0  if  the  manipulators  links  are 
outside  the  object.  From  (24),  it  can  be  shown  that  the  initial 
conditions  of  the  manipulator  and  the  bounding  constants 
can  be  selected  such  that  ya(t)  <  khi,  hence,  it  is 

clear  from  (18)  and  (19)  that  (3i(t)  >  OVt. 


The  result  of  Theorem  2  illustrates  that  the  repulsive  term 
ya(t)  can  be  bounded  by  an  exponentially  decreasing  func¬ 
tion.  This  means  that  when  all  the  manipulators  links  are  in 
contact  with  the  object  the  auxiliary  repulsion  function  ya(t) 
will  approach  a  constant  value  (X]?,*=1  khi),  hence  Pi(t)  w  0. 
Interestingly,  as  the  slack  in  the  robot  body  is  removed,  the 
effect  of  the  control  term  Um(f)  is  automatically  reduced. 
This  is  because  as  the  manipulator  links  make  contact  with 
the  object,  the  number  of  redundant  degrees  of  freedom 
available  to  accomplish  the  task  space  objective  reduces.  As 
a  consequence,  the  self-motion  component  of  the  control 
input  becomes  almost  zero  (i.e.,  the  null  space  projection 
||  (In  —  Jf  Jn)  |  approaches  zero),  and  hence,  (25)  is  no 
longer  satisfied. 

IV.  Grasping  with  Dynamic  Control 

In  the  previous  section,  a  kinematic  control  development 
was  presented  which  enabled  the  whole  arm  grasping  ob¬ 
jective  to  be  encoded  as  a  desired  trajectory  signal  which 
can  be  fed  to  the  subsequently  designed  joint  space  tracking 
controller.  This  desired  trajectory  signal  encodes  informa¬ 
tion  from  the  two  auxiliary  controllers,  the  end-effector 
positioning  controller,  and  the  body  self-motion  positioning 
controller.  In  the  subsequent  section,  the  kinematic  control 
will  be  utilized  to  generate  a  bounded  desired  joint  trajectory 
such  that  its  higher  order  derivatives  are  also  bounded. 

A.  Desired  Trajectory  Generator 

Traditionally  for  torque  based  control,  the  desired  tra¬ 
jectory  and  its  higher  order  derivatives  are  required  for 
the  control  implementation.  It  is  assumed  that  the  desired 
trajectory  and  its  higher  order  derivatives  are  always  bounded 
for  this  problem  to  be  tractable.  In  this  section,  a  desired 
trajectory  filter  which  generates  bounded  desired  joint  space 
trajectories  for  the  joint  space  tracking  controller  is  provided. 
The  structure  of  the  desired  trajectory  generator  is  motivated 
by  the  choice  of  the  joint  space  controller  [26],  which 
is  a  continuous,  nonlinear  integral  feedback  controller  and 
requires  the  desired  trajectory  to  be  bounded  upto  its  fourth 
derivative.  This  controller  was  selected  because  of  its  ability 
to  meet  the  tracking  objective  in  the  presence  of  system 
uncertainties  (i.e.  uncertainty  in  the  robot  dynamic  model 
and  unmeasurable  contact  forces). 

To  ensure  that  the  desired  joint  space  velocity  trajectory 
is  bounded,  we  could  use  the  following  expression 

qd  (t)  4  sat  (RHS  of  (9))  (27) 

where  RHS  denotes  the  right  hand  side  of  the 
equation,  sat(£)  £  R™  is  defined  as  sat(£)  = 
[saf(£i), saf(£2), •  •  •  ,saf(£„)]T  ,£„]T  € 

W1  where  satfa)  G  1  Vi  =  1,***  ,n  is  the  following 
saturation  function 

Cmin  if  Ci  —  £ min 

£ i  if  ^  £ min  Or  ^  £max 

Cmax  if  £,i  ^  £,max 


where  fmin ,  frnax  £  R+  are  constants.  If  (27)  is  used  to 
generate  the  desired  trajectory,  we  cannot  prove  that  gfif)  is 
bounded,  so  we  could  use  the  following  filtering  operation 

Qd  (s)  =  -jT^jsat  (RHS  of  (9»  (28) 

where  s  £  C  is  the  standard  Laplace  variable,  and  e  £  R+  is 
an  integration  constant  selected  very  close  to  zero.  However, 
in  the  case  of  (28),  we  cannot  prove  that  the  higher  order 
derivatives  of  qd(t)  will  remain  bounded.  So  the  desired 
trajectory  qd(t)  for  the  manipulator  joint  angles  are  generated 
by  the  following  expression 

qd  (fi)  4  7 - 77 - 73  ■ sat  (RHS  of  (9))  (29) 

(f  +  l)(f +  1 ) 

where  k  £  R+  is  an  integration  constant  selected  to  be  very 
large.  From  (29),  it  is  clear  that  qd  (t) ,  qd  (t) ,  qd  (t) ,  Qd  (t) , 
and  q  d  (t)  €  Coo- 

B.  Control  Objective 

The  objective  of  the  closed-loop  system  is  to  ensure 
asymptotic  tracking  between  the  manipulator  and  the  desired 
trajectory  in  the  sense  that 

q{t)  qd{f)  as  t  ->  oo  (30) 

where  qd(t)  £  R™  is  obtained  from  (29).  To  quantify  the 
control  objective,  an  error  signal  e\ (t)  £  Rn  is  defined  as 
follows 

ei  =  qd-q-  (31) 

Furthermore,  a  tracking  error  signal  e2  (t)  £  R"  is  defined 
as  follows 

e2  =  ei  +  7iei  (32) 

where  71  £  R+  is  a  control  gain. 

C.  Control  Law 

Since  the  robot  dynamic  model  is  a  nonlinear  uncertain 
multi-input  multi-output  system,  the  strategy  developed  in 
[26]  can  be  used  for  the  continuous  joint  space  controller. 
The  control  objective  of  (30)  can  be  met  with  the  following 
controller  [26] 

r  =  (Ks  +  In)  e2(t)  -  e2(t0)  +72  [  e2(r)dr 

Jt0 

+  f  [Tsgn(e2(T))]dT  (33) 

Jto 

where  r(f)  £  ln  is  the  control  input  defined  in  (3),  Ks, 
r  £  R”xn  are  positive  diagonal  control  gain  matrices, 
and  sgn(-)  £  R”  denotes  the  vector  signum  function  de¬ 
fined  as  sgn( £)  =  [s0n(£i),  sgn(&),  ■  ■  ■  ,sgn(fn)]T  = 
•'  )Cn]T  G  R".  The  controller  presented  in  (33), 
provides  asymptotic  convergence  of  the  joint  tracking  error, 
i.e.  ||ei (t)  ||  — >  0  as  t,  — >  00.  For  a  detailed  analysis  of  the 
controller  the  reader  is  referred  to  [26]. 

Remark  5:  The  trajectory  generator  defined  in  (29)  gen¬ 
erates  a  filtered  version  of  (9).  This  filtered  signal  is  used 


as  a  desired  trajectory  for  the  joint  space  controller  defined 
in  (33).  The  joint  space  controller  (33),  forces  the  actual 
robot  joint  angles  to  track  the  filtered  desired  trajectory  of 
(29).  However,  we  cannot  show  that  the  actual  robot  joint 
velocities  track  the  kinematic  velocity  signal  defined  in  (9). 
Thus,  the  results  of  Theorem  1  and  Theorem  2  may  not  be 
technically  valid,  however,  the  validity  of  the  approach  is 
illustrated  through  the  experimental  results  presented  in  the 
Appendix. 

V.  Conclusion 

A  whole  arm  grasping  controller  for  redundant  robot  ma¬ 
nipulators  was  presented.  A  kinematic  control  which  enables 
end-effector  position  tracking  information  as  well  as  body 
self-motion  positioning  control  information  to  be  encoded 
in  the  desired  trajectory  was  developed.  Then,  a  tracking 
controller,  developed  in  [26],  which  forces  the  robot  to  track 
a  desired  trajectory  in  the  presence  of  system  uncertainties 
and  unmeasurable  contact  forces  was  utilized.  The  controller 
provides  asymptotic  tracking  which  enables  the  whole  arm 
grasping  objective  to  be  completed.  Experimental  results 
for  a  planar,  three  link  configuration  of  the  Barrett  WAM 
are  provided  to  demonstrate  the  controller  performance. 
Future  work  will  include  applying  the  whole  arm  grasping 
technique  to  a  hyper-redundant  tentacle  manipulator  [13] 
under  development  at  Clemson  University. 
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Appendix  I 

Experimental  Results 

The  proposed  controller  was  implemented  on  three  links 
of  the  Barrett  whole  arm  manipulator  (WAM).  The  WAM 
is  a  seven  degree  of  freedom  (d.o.f.),  highly  dexterous  and 
back-drivable  robotic  manipulator.  To  simplify  the  controller 
implementation,  four  joints  of  the  robot  were  locked  at  fixed 


Fig.  1.  Experiment  setup  showing  the  Barrett  Whole  Arm  Manipulator 
and  object  to  the  grasped. 


angles  and  the  remaining  links  of  the  manipulator  were  used 
as  a  three  d.o.f.  planar  robot  manipulator.  Figure  1,  shows 
the  experimental  setup  for  a  circular  object  to  be  grasped. 

The  control  algorithm  was  written  in  “C++”  and  hosted  on 
an  AMD  Athlon  1.2  GHz  PC  running  the  QNX  6.2.1  real¬ 
time  operating  system.  Data  logging  and  on  line  gain  tuning 
was  performed  using  Qmotor  3.0  control  software  [11].  Data 
acquisition  and  control  implementation  was  performed  at  a 
frequency  of  1 .0  [kHz]  using  the  ServoToGo  I/O  board.  Joint 
positions  were  measured  using  the  optical  encoders  located 
at  the  motor  shaft  of  each  axis.  Joint  velocity  measurements 
were  obtained  using  a  filtered  backwards  difference  algo¬ 
rithm. 

Refer  to  Figure  2  for  explanation  of  the  notions  used  in 
this  section.  Xc  =  [xc,yc]T  £  R2  represents  the  co-ordinates 
of  the  center  of  the  object  and  ro  £  R  represents  the  object 
radius.  We  define  the  task  space  variable  for  each  of  the 
three  links  and  the  mid-point2  of  each  of  the  three  links  as 
Xi  =  [xi,yi\T  £  R 2  Vi  =  1,2,. .,6.  The  joint  angles  for 
the  three  links  are  represented  by  q  =  [q\,  <72,  93] T  £  R3  • 
The  object  specific  functions  defined  for  each  of  the  three 
links  and  the  mid-points  of  the  three  links  are  defined  as 

/3i(-)>  /fe(‘)>  "  ’  > /%>(•)  £  ®L 

The  object  specific  functions  for  this  planar  application 
were  defined  as  follows 

(3i  ( Xi )  =  (xi  -  xc )2  +  (t/i  -  ycf  -  r2  Vi  =  1,  ..,6.  (34) 


The  following  task-space  velocity  field  for  a  planar,  circular 
contour  was  utilized  [6] 


Xd  =  0(X6) 


-2  K(X6)f(X6) 
+2  c(X6) 


(X6  -  Xc) 

.  (ye  -  Vc) 

-(ye  -  Vc) 
(x6  -  xc) 


(35) 


where  the  functions  f(X&),  K(Xq ),  and  c(Xq)  E  M  are 


2 The  object  function  for  the  mid-points  of  each  of  the  manipulator  links 
was  used  for  this  three  link  application,  since  it  provides  more  control  over 
the  body  self-motion  positioning  control  (i.e.  the  repulsion  functions)  than 
just  using  the  joint  positions  alone. 


Fig.  2.  Planar  configuration  for  the  three  link  robot  with  a  circular  object. 


an  estimate  of  how  close  the  manipulators  links  are  to  the 
object.  For  the  experiment,  we  stop  the  trajectory  generation 
by  setting  Xd  =  0  when  ||/3(-)||  <  770,  where  the  constant 
77 0  =  0.01  was  determined  experimentally. 

Remark  6:  The  value  of  ||/3(-)||  at  which  we  stop  the 
generation  of  the  desired  trajectory  is  specific  to  a  particular 
grasp  configuration.  It  will  change  if  the  object  is  re¬ 
positioned  in  the  task  space.  However,  if  we  use  a  highly 
redundant  robot  arm  which  can  wrap  its  entire  body  around 
the  object,  then  ||/3(-)||  will  approach  zero  when  the  arm 
grasps  the  object,  since  the  entire  body  of  the  arm  will  be 
in  contact  with  the  object. 

Figure  3  shows  the  actual  and  desired  joint  angles.  Figure 
4  and  Figure  5  show  the  joint  space  tracking  error  and  the 
joint  control  torques  respectively.  Figure  6  show  the  spatial 
position  of  each  of  the  links  and  their  mid-points  as  defined 
in  Figure  2. 


defined  according  to  [6]  as  follows 

f(X6)  =  (x6  -  Xc )2  +  (t/6  -  Ucf  -  Tq 

k* 

K(X6)  = 


^ 0 


df(X6) 

dX6 


c(X6)  = 
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c0  exp  (-po  \J f 2  (A'e)) 
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df(Xe) 

dX6 

In  (36),  the  constant  parameters  were  selected  as  eo  =  0.005 
[m3],  po  =  20  [m-1],  fcg  =  0.1  [ms-1],  and  Co  = 
0.1  [ms-1].  The  desired  position  for  the  end-effector  is 
Xd  =  [xd,yd)T  €  R2.  The  controller  defined  in  (10)  was 
implemented  with  e  =  Xd  —  Xq,  V(Xd)  =  4||Xd||2,  and 
P(0  =  I- 

The  initial  joint  angles  were  q\  (0)  =  98[deg],  <72(0)  = 
45.8[deg],  (73(0)  =  31[deg],  which  corresponds  to  a  position 
of  £6(0)  =  0.368  [m]  j/6 (0)  =  —0.883  [m]  for  the  end- 

effector  in  the  task  space.  The  position  of  the  object  center  in 
the  task  space  was  found  to  be  xc  =  0.307  [m],  yc  =  —0.117 
[m],  the  radius  of  the  circular  object  was  found  to  be  0.12 
[m].  To  take  into  account  the  width  of  the  manipulator  arm, 
the  radius  of  the  object  was  set  to  ro  =  0.16  [m]  in  the 
implementation  of  the  repulsion  function.  The  control  gains 
were  selected  as  follows 

Ke  =  diag{800,  800},  kn  =  1,  km  =  100, 

kh  =  {0.001, 0.01, 0.01, 0.05, 8.5, 8}, 

=  {3, 3, 3, 3, 5, 5},  Ks  =  diag{16, 9, 6}, 

P  =  diag{ 5,  5,  2},  71  =  diag{  1, 1, 1}, 

72  =  diag{2 ,  2,  2},  e  =  0.01,  n  =  500 

Since  the  desired  trajectory  for  the  end-effector  is  a 
velocity  field,  it  will  continuously  generate  the  trajectory. 
To  stop  the  desired  trajectory  generation  when  all  the  links 
of  the  manipulator  make  contact  with  the  object,  the  norm 
of  the  following  vector  /?(■)  =  [/3i(-),/32(-)i  •  •  •  ,  /?e(-)]  e 
was  used.  As  the  links  of  the  manipulator  move  closer  to 
the  object  boundary,  ||/?(-)||  approaches  zero,  and  this  gives 
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Fig.  3.  Desired  joint  angles  qd(t)  and  actual  joint  angles  q(t). 
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Fig.  4.  Joint  space  position  tracking  error  e\(t). 


Y  axis  [ml  Y  axis  [ml  Y  axis 


Link  1 


-20 ' - 1 - * - * - 1 - 

0  5  10  15  20  25 

Link  2 


Time  [sec] 


Fig.  5.  Joint  space  control  torques  r(t). 
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Fig.  6.  Spatial  position  Xi  Vi  =  1,  •  • 


6  (each  link  and  mid-point  of  the  link). 


